


#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
#include "semphr.h"

#include "driverI2C_ISR.h"
#include "rtos_queues.h"
#include "driverLog.h"
#include "sensors.h"

#include "types.h"
#include "state.h"
#include "driverDSMX_UART_ISR.h"
#include "main_peripherals.h"

#include "sensors_HMC5883.h"
#include "sensors_MPL3115.h"
#include "sensors_MPU6050.h"


struct i2cAction_t       setupAction;
BYTE                     sensorWriteBuffer[2];
BYTE                     sensorReadBuffer;
BYTE        			sensorErrorValue;

struct i2cAction_t * sensorWrite(unsigned char address, unsigned char reg, unsigned char value)
{
    sensorWriteBuffer[0] = reg;
    sensorWriteBuffer[1] = value;

    setupAction.address = address;
    setupAction.writePacket = sensorWriteBuffer;
    setupAction.writeLength = 2;
    setupAction.readLength = 0;

    i2cQueueAction(&setupAction);

    int timeout = 1000;

    while (setupAction.state < I2C_STATE_DONE && timeout-- > 0)
    {
        vTaskDelay(1);
    }

    if (timeout <= 0)
    {
        i2cChannelReset(1);
        setupAction.state = I2C_STATE_ERROR;
    }

    return &setupAction;
}

struct i2cAction_t * sensorRead(unsigned char address, unsigned char reg, unsigned char errorValue)
{
    sensorWriteBuffer[0] = reg;

    setupAction.address = address;
    setupAction.writePacket = sensorWriteBuffer;
    setupAction.writeLength = 1;
    setupAction.readPacket = &sensorReadBuffer;
    setupAction.readLength = 1;

    i2cQueueAction(&setupAction);

    int timeout = 1000;

    while (setupAction.state < I2C_STATE_DONE && timeout-- > 0)
    {
        vTaskDelay(1);
    }

    if (timeout <= 0)
    {
        i2cChannelReset(1);
        setupAction.state = I2C_STATE_ERROR;
    }

    return &setupAction;
}

void sensorsInit()
{
    magnSetup();
	baroSetup();
    mpu6050Setup();
}
